#pragma once

#ifndef _FRONTEND_H_
#define _FRONTEND_H_

#include "frame.h"
#include"config.h"
#include"map.h"
#include"imu_handle.h"

namespace esikf_slam{

enum class  FrontendStatus { INITING, TRACKING_GOOD, TRACKING_BAD, LOST };

class Frontend{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<Frontend> Ptr;

    Config::Ptr p_config_;
    Frame::Ptr p_last_frame_,p_current_frame_;

    Map::Ptr p_map_;

    Frontend(){}
    Frontend(Config::Ptr p_config){
        p_config_ = p_config;
        p_map_.reset(new Map(p_config));
        p_imu_handle_.reset(new IMUHandle(p_config_));

        laserCloudOri_.reset(new pcl::PointCloud<PointType>);
        coeffSel_.reset(new pcl::PointCloud<PointType>);

        matP_ = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
    }

    void addFrame(shared_ptr<Frame> p_frame);

    bool frontendHandle();

private:
    FrontendStatus status_ = FrontendStatus::INITING;

    mutex frame_mutex_;
    deque<Frame::Ptr> frame_deq_;

    IMUHandle::Ptr p_imu_handle_;

    pcl::KdTreeFLANN<PointType>::Ptr p_kdtree_local_;
    pcl::PointCloud<PointType>::Ptr laserCloudOri_;
    pcl::PointCloud<PointType>::Ptr coeffSel_;
    double max_dis_plane_for_match_ = 1;
    State linear_state_,state_last_kf_;
    mutex coeff_mutex_;

    void init();

    void frameRegistration();
    void predictBatch();
    void calSurfResidual();

    bool  judgeKeyframe();

    bool LMOptimization(int iterCount);
    bool is_degenerate_ = false;
    cv::Mat matP_;
};



}
#endif  
